
from PidController import PidController
from levitationSys import levitationSys

import matplotlib.pyplot as plt
import numpy as np


if __name__ == '__main__':

 
    kp = 20000
    ki = 1000
    kd = 500
    ncount = 2000    # 循环次数
    h = 0.0002  #步长
    ts = np.zeros(ncount)
  
    # 被控对象
    sys = levitationSys(ncount, 0, 0, 0, 0)
    # 控制器
    PID = PidController(kp, ki, kd)
  
    gap = 0.0
    gap_last = 0.0

    for i in range(ncount):
        gap = sys.system_io(i, PID.pidUpdate(20 - gap_last, h), h, 0)
        ts[i] = i * h
        print(gap)
        gap_last = gap
    # 图像输出
    plt.plot(ts, sys.Y1)
    plt.xlabel('x-axis')
    plt.ylabel('y-axis')
    plt.title('Simple Line Plot')
    plt.show()
